#include "WPILib.h"
#include "Robot2489.h"

#define MOVEAWAYIMMEDIATELY 1
#define NEARZONE 2
#define MIDDLEZONE 3
#define FARZONE 4
#define NOTHINGTODO 5

	/**
	 * Let's assign someone to write code for this! (i.e. kick and then move 
	 * out of the way).
	 */

void Robot2489::Autonomous(void)
	{
		int automode;
		GetWatchdog().SetEnabled(false);
		KarlKompressor->Start();
		if (!dseio->GetDigital(2))
		{
			automode=MOVEAWAYIMMEDIATELY;
		}
		if (!dseio->GetDigital(4))
		{
			automode=NEARZONE;
		}
		if (!dseio->GetDigital(6))
		{
			automode=MIDDLEZONE;
		}
		if (!dseio->GetDigital(8))
		{
			automode=FARZONE;
		}
		if (!dseio->GetDigital(1))
		{
			automode=NOTHINGTODO;
		}
		switch(automode)
		{
		case 1: //GTFO LOL
		{
			TurnRight();
			break;
		}
		case 2: //near
		{
			RetractKicker();	
			Wait(KICKERRETRACTTIME);
			ChargeKicker();
			Wait(KICKERREADYINGTIME);
			CloseLatch();
			DriveForward(.6, FORWARDDRIVETIME);
			kickFunction();
			Wait(1);
			TurnRight();
			break;
		}
		case 3: //middle
		{
			Wait(9);
			RetractKicker();	
			Wait(KICKERRETRACTTIME);
			ChargeKicker();
			Wait(KICKERREADYINGTIME);
			CloseLatch();
			DriveForward(.6, FORWARDDRIVETIME);
			kickFunction();
			Wait(1);
			DriveForward(.6, FORWARDDRIVETIME);
			kickFunction();
			Wait(1);
			TurnRight();
			break;
		}
		case 4: //far
		{
			Wait(9);
			RetractKicker();	
			Wait(KICKERRETRACTTIME);
			ChargeKicker();
			Wait(KICKERREADYINGTIME);
			CloseLatch();
			DriveForward(.6, FORWARDDRIVETIME);
			kickFunction();
			Wait(1);
			DriveForward(.6, FORWARDDRIVETIME);
			kickFunction();
			Wait(1);
			DriveForward(.6, FORWARDDRIVETIME);
			kickFunction();
			Wait(1);	
			TurnRight();
			break;
		}
		case 5:
		{
			KarlKompressor->Start();
			break;
		}
		}
		
	}
